from libs.PipeLine import PipeLine
from libs.AIBase import AIBase
from libs.AI2D import Ai2d
from libs.Utils import *
import os,sys,ujson,gc,math
from media.media import *
from media.sensor import *
import nncase_runtime as nn
import ulab.numpy as np
import image
import aidemo

# 自定义车牌检测类
class LicenceDetectionApp(AIBase):
    # 初始化函数，设置车牌检测应用的参数
    def __init__(self, kmodel_path, model_input_size, confidence_threshold=0.5, nms_threshold=0.2, rgb888p_size=[224,224], display_size=[1920,1080], debug_mode=0):
        super().__init__(kmodel_path, model_input_size, rgb888p_size, debug_mode)  # 调用基类的初始化函数
        self.kmodel_path = kmodel_path  # 模型路径
        # 模型输入分辨率
        self.model_input_size = model_input_size
        # 分类阈值
        self.confidence_threshold = confidence_threshold
        self.nms_threshold = nms_threshold
        self.max_boxes_num=10
        # sensor给到AI的图像分辨率
        self.rgb888p_size = [ALIGN_UP(rgb888p_size[0], 16), rgb888p_size[1]]
        # 显示分辨率
        self.display_size = [ALIGN_UP(display_size[0], 16), display_size[1]]
        self.debug_mode = debug_mode
        # Ai2d实例，用于实现模型预处理
        self.ai2d = Ai2d(debug_mode)
        # 设置Ai2d的输入输出格式和类型
        self.ai2d.set_ai2d_dtype(nn.ai2d_format.NCHW_FMT, nn.ai2d_format.NCHW_FMT, np.uint8, np.uint8)

    # 配置预处理操作，这里使用了pad和resize，Ai2d支持crop/shift/pad/resize/affine
    def config_preprocess(self, input_image_size=None):
        with ScopedTiming("set preprocess config", self.debug_mode > 0):
            # 初始化ai2d预处理配置，默认为sensor给到AI的尺寸，可以通过设置input_image_size自行修改输入尺寸
            ai2d_input_size = input_image_size if input_image_size else self.rgb888p_size
            top, bottom, left, right,_ =letterbox_pad_param(self.rgb888p_size,self.model_input_size)
            self.ai2d.pad([0, 0, 0, 0, top, bottom, left, right], 0, [128, 128, 128])  # 填充边缘
            self.ai2d.resize(nn.interp_method.tf_bilinear, nn.interp_mode.half_pixel)
            self.ai2d.build([1,3,ai2d_input_size[1],ai2d_input_size[0]],[1,3,self.model_input_size[1],self.model_input_size[0]])

    # 自定义当前任务的后处理
    def postprocess(self, results):
        with ScopedTiming("postprocess", self.debug_mode > 0):
            new_result=results[0][0].transpose()
            det_res = aidemo.yolo_license_plate_det_postprocess(new_result.copy(),[self.rgb888p_size[1],self.rgb888p_size[0]],[self.model_input_size[1],self.model_input_size[0]],[self.display_size[1],self.display_size[0]],self.confidence_threshold,self.nms_threshold,self.max_boxes_num)
            return det_res

    # 绘制检测结果到屏幕上
    def draw_result(self, pl, dets):
        with ScopedTiming("display_draw", self.debug_mode > 0):
            if dets:
                pl.osd_img.clear()  # 清除屏幕
                kps=dets[0]
                boxes=dets[1]
                scores=dets[2]
                for i in range(len(kps)):
                    for j in range(len(kps[i])):
                        if j%2==0:
                            kps[i][j]=kps[i][j]*self.display_size[0]/self.rgb888p_size[0]
                        else:
                            kps[i][j]=kps[i][j]*self.display_size[1]/self.rgb888p_size[1]
                    for j in range(len(boxes[i])):
                        if j%2==0:
                            boxes[i][j]=boxes[i][j]*self.display_size[0]/self.rgb888p_size[0]
                        else:
                            boxes[i][j]=boxes[i][j]*self.display_size[1]/self.rgb888p_size[1]
                    # 在屏幕上绘制检测框
                    x1,y1,x2,y2,x3,y3,x4,y4= map(lambda x: int(round(x, 0)), kps[i])
                    x, y, w, h =map(lambda x: int(round(x, 0)), boxes[i])
                    pl.osd_img.draw_rectangle(x,y, w, h, color=(255,0,0,255),thickness=4)
                    pl.osd_img.draw_circle(x1, y1, 5, color=(255, 0, 255, 0),fill=True)
                    pl.osd_img.draw_circle(x2, y2, 5, color=(255, 0, 255, 0),fill=True)
                    pl.osd_img.draw_circle(x3, y3, 5, color=(255, 0, 255, 0),fill=True)
                    pl.osd_img.draw_circle(x4, y4, 5, color=(255, 0, 255, 0),fill=True)
            else:
                pl.osd_img.clear()  # 如果没有检测结果，则清空屏幕

if __name__=="__main__":
    # 添加显示模式，默认hdmi，可选hdmi/lcd/lt9611/st7701/hx8399/nt35516,其中hdmi默认置为lt9611，分辨率1920*1080；lcd默认置为st7701，分辨率800*480
    display_mode="lcd"
    # k230保持不变，k230d可调整为[640,360]
    rgb888p_size = [640, 360]
    # 模型路径
    kmodel_path="/sdcard/examples/kmodel/yolo_license_plate_det.kmodel"
    # 其它参数设置
    confidence_threshold = 0.25
    nms_threshold = 0.45
    # 初始化PipeLine
    pl=PipeLine(rgb888p_size=rgb888p_size,display_size=[800,480],display_mode=display_mode)
    pl.create(sensor_id=2,crop_vertical=False)
    display_size=pl.get_display_size()
    # 初始化自定义车牌检测实例
    licence_det=LicenceDetectionApp(kmodel_path,model_input_size=[640,640],confidence_threshold=confidence_threshold,nms_threshold=nms_threshold,rgb888p_size=rgb888p_size,display_size=display_size,debug_mode=0)
    licence_det.config_preprocess()
    while True:
        with ScopedTiming("total",1):
            # 获取当前帧数据
            img=pl.get_frame()
            # 推理当前帧
            res=licence_det.run(img)
            # 绘制结果到PipeLine的osd图像
            licence_det.draw_result(pl,res)
            # 显示当前的绘制结果
            pl.show_image()
            gc.collect()
    licence_det.deinit()
    pl.destroy()

